#include <actionlib/client/simple_action_client.h>
#include <actionlib_test/DoDishesAction.h>
 
typedef actionlib::SimpleActionClient<actionlib_test::DoDishesAction> Client;
static Client *client_ptr;

// 自定义函数提取文件名
std::string extractFileName(const std::string& path) {
    size_t pos = path.find_last_of("\\/");
    return path.substr(pos +1);
}

static void feedback_callback(const actionlib_test::DoDishesFeedbackConstPtr &feedback){
	printf("%s(%s): percent_complete=%f\n", extractFileName(__FILE__), __FUNCTION__, feedback->percent_complete);
	if ((int)feedback->percent_complete == 30)
		client_ptr->cancelGoal();
}

int main(int argc, char **argv){
	ros::init(argc, argv, "do_dishes_client");
	Client client("do_dishes", true);
	client_ptr = &client;
	client.waitForServer();
	
	actionlib_test::DoDishesGoal goal;
	goal.dishwasher_id = 50000;
	client.sendGoal(goal, Client::SimpleDoneCallback(), Client::SimpleActiveCallback(), feedback_callback);
	
	bool succeeded = client.waitForResult(ros::Duration(20.0));
	if (succeeded){
		actionlib_test::DoDishesResultConstPtr result = client.getResult();
		printf("Yay! %u dishes are now clean.\n", result->total_dishes_cleaned);
	}
	else
		printf("timeout!!!!\n");
		
	printf("Current State: %s\n", client.getState().toString().c_str());
	return 0;
}
 